#include "./flash.h"
#include "../device/compat.h"

namespace driver::flash {

static constexpr uint32_t FLASH_KEY1 = 0x45670123;
static constexpr uint32_t FLASH_KEY2 = 0xCDEF89AB;

static void UnLock() {
    FLASH->KEYR = FLASH_KEY1;
    FLASH->KEYR = FLASH_KEY2;
}

static inline void Lock() {
    FLASH->CR |= 0x80000000;
}

static const Sector AddrInfos[] = {
    { Sector0Addr, Sector0Size },
    { Sector1Addr, Sector1Size },
    { Sector2Addr, Sector2Size },
    { Sector3Addr, Sector3Size },
    { Sector4Addr, Sector4Size },
    { Sector5Addr, Sector5Size },
    { Sector6Addr, Sector6Size },
    { Sector7Addr, Sector7Size },
};

static constexpr uint32_t SectorCount = sizeof (AddrInfos) / sizeof (AddrInfos[0]);

static void delay() {
    for (volatile uint32_t i = 0; i < 1000; i ++);
}

static uint32_t sectorIndexByAddr(uint32_t addr) {
	for (uint32_t i = 0; i < SectorCount; i ++) {
		const Sector &info = AddrInfos[i];
		if ((addr >= info.Addr) && (addr < (info.Addr + info.Size))) {
			return i;
		}
	}
	return 0;
}

static uint32_t status() {
    uint32_t tmp = FLASH->SR;
    if (tmp & 0x10000) return 1; // BSY
    if (tmp & 0x10) return 2; // 写保护错误
    if (tmp & 0x20) return 2; // 编程错误1
    if (tmp & 0x40) return 2; // 编程错误2
    if (tmp & 0x80) return 2; // 编程错误3
    return 0;
}

static uint32_t waitDone(uint32_t timeout) {

    for (uint32_t i = 0; i < timeout; i ++) {
        uint32_t ret = status();
        if (ret != 1) {
            return ret;
        }
        delay();
    }
    return 3;
}

static bool sectorEraseById(uint32_t id) {
    uint32_t ret = 0;
    UnLock();
    FLASH->ACR &= ~(0x01 << 10);
    ret = waitDone(200000);
    if (ret != 0) {
        goto finish;
    }
	FLASH->CR &= ~(0x03 << 8);
	FLASH->CR &= 2 << 8;
	
	FLASH->CR &= ~(0x1F << 3);
	FLASH->CR |= id << 3;
	
	FLASH->CR |= 0x01 << 1;
	FLASH->CR |= 0x01 << 16;
	
	ret = waitDone(200000);
	if (ret != 0x01) {
		FLASH->CR &= ~(0x01 << 1);
	}
finish:
    FLASH->ACR |= 0x01 << 10;
    Lock();

    return ret == 0x00;
}

static bool write(uint32_t addr, uint32_t val) {
    uint32_t ret = waitDone(255);
	if (ret == 0x00) {
		FLASH->CR &= ~(0x03 << 8);
		FLASH->CR |= 0x02 << 8;
		FLASH->CR |= 0x01;
		*((volatile uint32_t *) addr) = val;
		ret = waitDone(255);
		if (ret != 0x01) {
			FLASH->CR &= ~0x01;
		}
	}
	return *((volatile uint32_t *) addr) == val;
}

bool Erase(uint32_t addr, uint32_t size) {
	uint32_t index = 0;
	uint32_t baseId = sectorIndexByAddr(addr);
	for (;index < size;) {
		if (baseId >= SectorCount) {
			return false;
		}
		if (!sectorEraseById(baseId)) {
			return false;
		}
		index += AddrInfos[baseId].Size;
		baseId ++;
	}
	return true;
}

bool Write(uint32_t addr, const void *data, uint32_t size) {
	auto pData = reinterpret_cast<const uint32_t *>(data);
	bool ret = false;
	size = (size + 3) / 4;
	
	UnLock();
	for (uint32_t index = 0; index < size; index ++) {
		ret = write(addr, pData[index]);
		if (!ret) {
			goto finish;
		}
		addr += 4;
	}
finish:	
	Lock();
	return ret;
}

void Read(uint32_t addr, void *data, uint32_t size) {
	auto *dest = reinterpret_cast<uint8_t *>(data);
	auto *src = reinterpret_cast<const uint8_t *>(addr);
	for (uint32_t i = 0; i < size; i ++) {
		dest[i] = src[i];
	}
}

}


